import cv2
import numpy as np
import math
import camera_control as dh

# 读取图像
carb_xml = 'tinyPara.xml'
fs = cv2.FileStorage(carb_xml, cv2.FILE_STORAGE_READ)
intrinsics = fs.getNode('camera-matrix').mat()
distortion = fs.getNode('distortion').mat().T
cam_test = dh.Camera(dh.CameraSN.tiny)
cam_test.open()
cam_test.set_exp(60000)
cam_test.stream('on')

font = cv2.FONT_HERSHEY_SIMPLEX  # 字体
font_scale = 0.8  # 字体缩放
color = (255, 255, 0)  # BGR颜色
thickness = 1  # 线条粗细
line_type = cv2.LINE_AA  # 线型，LINE_AA表示抗锯齿
while True:
    image = cam_test.get_pic()
    # 将图像转换为灰度图
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

    # 定义AprilGrid的参数
    grid = cv2.aruco.GridBoard(
        size=(6, 6),
        markerLength=22,  # AprilGrid标签的边长（单位可以是米）
        markerSeparation=6.6,  # 标签之间的间隔（单位可以是米）
        dictionary=cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_APRILTAG_36h11)  # 使用的标签类型
    )

    # 检测AprilGrid
    corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
        gray, cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_APRILTAG_36h11)
    )

    if ids is not None:
        # 检测到AprilGrid，绘制并标记检测到的格点
        cv2.aruco.drawDetectedMarkers(image, corners, ids)
        cv2.imshow('AprilGrid Detection', image)
        cv2.waitKey(1)
    else:
        cv2.imshow('AprilGrid Detection', image)
        cv2.waitKey(1)

